/* ========================================
 *
 * Copyright YOUR COMPANY, THE YEAR
 * All Rights Reserved
 * UNPUBLISHED, LICENSED SOFTWARE.
 *
 * CONFIDENTIAL AND PROPRIETARY INFORMATION
 * WHICH IS THE PROPERTY OF your company.
 *
 * ========================================
*/

/* [] END OF FILE */


#include <stdlib.h>

/* Scheduler include files. */
#include "FreeRTOS.h"
#include "task.h"

/* Demo program include files. */
#include "project.h"
#include "main.h"
#include "pedo.h"
#include "mma845x.h"
#include "stepcount.h"

portTASK_FUNCTION( vPedoTask, pvParameters )
{
    uint8 raw_data[6];
    uint8 process_flag = 0;

    int16 gsensor_x, gsensor_y, gsensor_z;

    unsigned long V_OldStepCounter_U32 = 0;
    unsigned long V_NewStepCounter_U32 = 0;

    unsigned char stepActivity = 0;

    UART_DEB_1_PutString("Hello\r\n");
    UART_DEB_1_PutString("This is demo program for Pedometer\r\n");


    mma845x_writereg(MMA845x_CTRL_REG1,0x00);
    mma845x_writereg(MMA845x_CTRL_REG1,0x20);//50hz 
    mma845x_writereg(MMA845x_CTRL_REG1,0x21);



    InitAlgo();
    resetStepCount();
    enableRobustness();
    stopDetection();
    startDetection();

    UART_DEB_1_PutString("init success\r\n");
    while(1)
    {
        vTaskDelay(10);
        if(mma845x_readreg(MMA845x_STATUS) != 0x00)
        {
            mma845x_read_data(raw_data);
            process_flag++;
            if(process_flag > 0)
            {
                process_flag = 0;

                gsensor_x = ((raw_data[0] << 8) & 0xFF00) | raw_data[1];
                gsensor_y = ((raw_data[2] << 8) & 0xFF00) | raw_data[3];
                gsensor_z = ((raw_data[4] << 8) & 0xFF00) | raw_data[5];


                gsensor_x = gsensor_x >> 6;
                gsensor_y = gsensor_y >> 6;
                gsensor_z = gsensor_z >> 6;
                
                //printf("\t\t\t\t%d\t%d\t%d\r\n",gsensor_x, gsensor_y, gsensor_z);

                processAccelarationData(gsensor_x, gsensor_y, gsensor_z);

                V_NewStepCounter_U32 = getStepCount();

                if(V_NewStepCounter_U32 != V_OldStepCounter_U32)
                {
                    stepActivity = getActivity();
                    printf("Step count is %ld\t", V_NewStepCounter_U32);
                    if(stepActivity == 0x11)
                        printf("In Walk\r\n");
                    if(stepActivity == 0x10)
                        printf("In Slow Walk\r\n");
                    if(stepActivity == 0x12)
                        printf("In Jog\r\n");
                    V_OldStepCounter_U32 = V_NewStepCounter_U32;
                }
            }
        }

    }
}

void vStartPedoTasks( UBaseType_t uxPriority )
{
    /* Create the three tasks. */
    /* Spawn the task. */
    xTaskCreate( vPedoTask, "PedoTask", 500, NULL, uxPriority, ( TaskHandle_t * ) NULL );
}